/****************************************************************************
*
*					Functions of class LoopyBP
*
****************************************************************************/

#include <string>
#include <vector>
#include <stdlib.h>
#include <cstdlib>
#include "LoopyBP.h"

// Parameter sets how often to make vector resize - once per this number of resize calls
#define vectorMinResize		10
#define eps					0.001


/****************************************************************************
*					LoopyBP::LoopyBP
*
*	Description:	Standard constructor with initialization of internal parameters
*	Parameters:		-
*	Returns:		-
*
****************************************************************************/
LoopyBP::LoopyBP (void) {
	// defult value for dumping parameter
	lambda = 1.0; // == 1 -means no dumping
	acceptableDeviation = 1.0; // 0.0 - means only full coincidence in beliefs over one sepSet is accepted as convergence
	lastRunConvergence = false; // no previous runs of inference
}


/****************************************************************************
*					LoopyBP::~LoopyBP
*
*	Description:	Standard destructor with removing all dynamic internal variables
*	Parameters:		-
*	Returns:		-
*
****************************************************************************/
LoopyBP::~LoopyBP (void) {
	
}


/****************************************************************************
*					LoopyBP::run
*
*	Description:	Runs inference in the given PGM
*	Parameters:		Z - partition function value set by this inference run
*					pgmStruct - description of the PGM
*	Returns:		0
*
****************************************************************************/
int LoopyBP::run (float& logZ, PGMStruct& pgmStruct) {
	// Creating a cluster graph from pgmStruct
	clusterGraph.initStructure(pgmStruct);

	// Preparing some variables
	unsigned int seqID, sepSetSize;
	sepSetSize = clusterGraph.sepSets.size();
	if (sepSetSize > 0) {
		// there is at least one sepSet
		sepSetSeq.resize(sepSetSize);
		isSepSetFree.resize(sepSetSize, true);
		isDomainFree.resize(clusterGraph.domainDescriptions.size(), true);
		unsigned int i, j = 0;
		lastMASEHolds = 0;
		lastMASE = FLT_MAX;
		// Main Cycle that goes on until convergence
		while (selectSepSet (sepSetSeq[0], pgmStruct)) {
			// printout statistics:
			j++;
			//std::cout << "\n\tLoopy Belief Propagation: iteration number - " << j << ".";
			// prepare variables that store the order of propagating messages
			seqID = 0;
			for (i = 0; i < sepSetSize; i++)
				isSepSetFree[i] = true;
			isSepSetFree[sepSetSeq[0]] = false; 
			for (i = 0; i < isDomainFree.size(); i++)
				isDomainFree[i] = true;
			isDomainFree[ clusterGraph.sepSets[sepSetSeq[0]].parOne ] = false;
			// run propagation recursively
			runPropagation(seqID, true, pgmStruct);
		}
	}
	
	// setting a variable showing that we've reached convergence
	lastRunConvergence = true;

	// setting all marginals to be from 0 to 1 (divide by partition functions)
	//  and setting partition function value
	logZ = (float) std::log (setZandNormalizeBeliefs ());
	
	return 0;
}


/****************************************************************************
*					LoopyBP::run
*
*	Description:	Runs inference conditional on a feature 'featureDescr'
*	Parameters:		expectedValue - expected value of the feature w.r.t. 
*						probability distribution defined by PGM
*					featureDescr - variables and their values (representation
*						of the simple feature)
*					pgmStruct - description of the PGM
*	Returns:		0
*
****************************************************************************/
int LoopyBP::run (float& expectedValue, const std::vector < std::pair<unsigned int, unsigned int> >& featureDescr, PGMStruct& pgmStruct) {
	return run (expectedValue, featureDescr, featureDescr.size(), pgmStruct);
}


/****************************************************************************
*					LoopyBP::run
*
*	Description:	Runs inference conditional on a feature 'featureDescr' 
*					with a given featureDescrSize
*	Parameters:		expectedValue - expected value of the feature w.r.t. 
*						probability distribution defined by PGM
*					featureDescr - variables and their values (representation
*						of the simple feature)
*					pgmStruct - description of the PGM
*	Returns:		0
*
****************************************************************************/
int LoopyBP::run (
		float& expectedValue,
		const std::vector < std::pair<unsigned int, unsigned int> >& featureDescr, 
		const unsigned int& featureDescrSize, 
		PGMStruct& pgmStruct) 
{
	// Checking correctness - whether the full version of inference was run prior to this run and were converged
	if (!lastRunConvergence) {
		std::cout << "Warning in LoopyBP::run: conditional inference could be run only after successful convergence of inconditional inference!\n";
		expectedValue = 0.0;
		return -1;
	}
	// Constructing a path in cluster graph that fully covers the feature
	std::vector < std::vector < std::pair<unsigned int, bool> > > path;
	std::vector <unsigned int> firstDomain;
	getMinPath (firstDomain, path, featureDescr, featureDescrSize, pgmStruct);
	// Propagating messages along the constructed ways
	expectedValue = 1.0;
	for (unsigned int i = 0; i < firstDomain.size(); i++)
		expectedValue *= runCondPropagation (firstDomain[i], path[i], featureDescr, featureDescrSize, pgmStruct);
	return 0;
}

/****************************************************************************
*					LoopyBP::run
*
*	Description:	Runs inference for evaluation of expected value of a diven 
*					feature
*	Parameters:		expectedValue - expected value of the feature w.r.t. 
*						probability distribution defined by PGM
*					feature - a feature for which to calculate expected value
*						w.r.t. probability distribution defined by pgmStruct
*					pgmStruct - description of the PGM
*	Returns:		0 - if everything went well
*					-1 - if an error occured
*
****************************************************************************/
int LoopyBP::run (float& expectedValue, Feature& feature, std::vector <unsigned int>& varIDs, PGMStruct& pgmStruct) {
	std::vector <unsigned int> vals; 
	feature.getDesiredValues(vals);
	unsigned int numOfVals;
	numOfVals = vals.size();
	if (numOfVals == 0) {
		// running inference in general case - exactly running inference for a given feature
		//  DUMMY - for future implementation
		std::cout << "Error in LoopyBP::run: dummy branch of the if clause is used.\n";
		return -1;
	} else {
		// checking that sizes are coinside
		if (numOfVals != varIDs.size()) {
			std::cout << "Warning in LoopyBP::run: number of variabls is not equal to the number of desired values.\n";
			expectedValue = 0.0;
			return -1;
		}
		std::vector < std::pair<unsigned int, unsigned int> > featureDescr;
		featureDescr.resize(numOfVals);
		for (unsigned int i = 0; i < numOfVals; i++) {
			featureDescr[i].first = varIDs[i];
			featureDescr[i].second = vals[i];
		}
		return run (expectedValue, featureDescr, pgmStruct);
	}
	// should not get here
	return -1;
}


/****************************************************************************
*					LoopyBP::getLogLikelihood
*
*	Description:	Return likelihood of the data in the data set
*	Parameters:		dataSet - a set of samples for which to calculate the likelihood
*					pgmStruct - a description of the PGM
*	Returns:		float - log likelihood of the data set w.r.t. pgm
*
****************************************************************************/
float LoopyBP::getLogLikelihood (DataSet& dataSet, PGMStruct& pgmStruct) {
	// Trivial case
	if (dataSet.data.size() < 1)
		return 0.0;
	float value, toRet = 0.0;
	unsigned int j, numOfVals = pgmStruct.nVars;
	std::vector < std::pair<unsigned int, unsigned int> > featureDescr;
	featureDescr.resize(numOfVals);
	for (j = 0; j < numOfVals; j++)
		featureDescr[j].first = j;
	for (unsigned int i = 0; i < dataSet.data.size(); i++) {
		for (j = 0; j < numOfVals; j++)
			featureDescr[j].second = dataSet.data[i][j];
		run (value, featureDescr, pgmStruct);
		toRet += std::log (value);
	}
	return toRet;
}


/****************************************************************************
*					LoopyBP::getMinPath
*
*	Description:	Returns the shortest path in cluster graph that covers fully 
*					the feature of interest
*	Parameters:		firstDomains - if some variables of the feature consist 
*						of one independend from others variable then this vector
*						stores the ID of the corresponding domain (no tree is stored)
*					path - a set of trees that define how to send messages in
*						factor graph in order to get the expectation for the feature
*					featureDescr - a set of variableIDs (and their values) that 
*						define a simple feature
*					pgmStruct - description of the PGM (of all its elements)
*	Returns:		int - number of trees in factor graph that cover the feature
*
****************************************************************************/
int LoopyBP::getMinPath(std::vector<unsigned int>& firstDomains, 
						std::vector < std::vector < std::pair<unsigned int, bool> > >& path, 
						const std::vector < std::pair<unsigned int, unsigned int> >& featureDescr, 
						const unsigned int& featureDescrSize,
						PGMStruct& pgmStruct) 
{
	firstDomains.resize(1);
	path.resize(1);
	std::vector < std::vector < std::pair<unsigned int, unsigned int> > > subFeatureDescr;
	getMinPath(firstDomains[0], path[0], subFeatureDescr, featureDescr, featureDescrSize, pgmStruct);
	unsigned int numGraphs = subFeatureDescr.size();
	if (numGraphs > 0) {
		firstDomains.resize(numGraphs);
		path.resize(numGraphs);
		std::vector < std::vector < std::pair<unsigned int, unsigned int> > > tmpSubFeatureDescr;
		for (unsigned int i = 0; i < numGraphs; i++) {
			getMinPath(firstDomains[i], path[i], tmpSubFeatureDescr, subFeatureDescr[i], subFeatureDescr[i].size(), pgmStruct);
		}
		return (int) numGraphs;
	}
	return 1;
}


/****************************************************************************
*					LoopyBP::getMinPath
*
*	Description:	Returns the shortest path in cluster graph that covers fully 
*					the feature of interest
*	Parameters:		firstDomains - if the feature consists of one variable then 
*						this is the ID of the corresponding domain (no tree is stored)
*					path - a trees that defines how to send messages in
*						factor graph in order to get the expectation for the feature
*					subFeatureDescr - if there is no tree that covers all variables
*						in the feature then this vector contains all connected subsets
*						of the feature variables
*					featureDescr - a set of variableIDs (and their values) that 
*						define a simple feature
*					pgmStruct - description of the PGM (of all its elements)
*	Returns:		1 - if a tree that covers all feature variables is created
*					2 - if there is no such tree and subFeatureDescr was defined
*
****************************************************************************/
int LoopyBP::getMinPath(unsigned int& firstDomain,
						std::vector < std::pair<unsigned int, bool> >& path, 
						std::vector < std::vector < std::pair<unsigned int, unsigned int> > >& subFeatureDescr, 
						const std::vector < std::pair<unsigned int, unsigned int> >& featureDescr, 
						const unsigned int& featureDescrSize,
						PGMStruct& pgmStruct) 
{
	unsigned int fSize = featureDescrSize;
	// removing contents of variables that should be set in this function
	path.clear();
	subFeatureDescr.clear();
	// Special case - when feature consists of one variable
	if (fSize == 1) {
		firstDomain = clusterGraph.oneVarDomain[featureDescr[0].first];
		return 0;
	}
	firstDomain = (unsigned int) -1;
	// some internal variables
	unsigned int i, j, curDomID;
	// saves info for each domain - what variables are accessible from it
	//        domainID                 what vars are accessible, at what min distance they are accessible
	std::map< unsigned int, std::pair <std::vector<bool>, std::vector<unsigned int> > > colors; 
	// Setting starting points - starting from all domains that consist of one variable (that is included in the feature of interest)
	//  vector of domainIDs whose labels were recently changed
	std::set <unsigned int> recentlyChanged, newlyChanged;
	std::vector<bool> coveredVars;
	coveredVars.resize(fSize, false);
	for (i = 0; i < fSize; i++) {
		curDomID = clusterGraph.oneVarDomain[featureDescr[i].first];
		coveredVars[i] = true;
		colors[curDomID].first = coveredVars;
		colors[curDomID].second.resize(fSize, 0);
		coveredVars[i] = false;
		recentlyChanged.insert(curDomID);
	}
	// cycle that tends to end up with one domain that could be used as a root of the tree that will connect all variables in the feature
	bool allTrue = false, changeDone = false;
	std::set<unsigned int>::const_iterator cIter;
	while (recentlyChanged.size() > 0) {
		newlyChanged.clear();
		for (cIter = recentlyChanged.begin(); cIter != recentlyChanged.end(); cIter++) {
			// coloring decendant domains of the *cIter with colors of the *cIter domain
			for (i = 0; i < clusterGraph.domainDescriptions[*cIter].neighbors.size(); i++) {
				// getting domainID of the descendant of *cIter
				if ((*cIter) == clusterGraph.sepSets[ clusterGraph.domainDescriptions[*cIter].neighbors[i] ].parOne) 
					curDomID = clusterGraph.sepSets[ clusterGraph.domainDescriptions[*cIter].neighbors[i] ].parTwo;
				else
					curDomID = clusterGraph.sepSets[ clusterGraph.domainDescriptions[*cIter].neighbors[i] ].parOne;
				// updating descendant coloring
				//  if this domain was not colored previously - creating a space for saving it's coloring
				if (colors[curDomID].first.size() < 1) {
					colors[curDomID].first.resize(fSize, false);
					colors[curDomID].second.resize(fSize, 0);
				}
				allTrue = true;
				changeDone = false;
				for (j = 0; j < fSize; j++) {
					if ((colors[*cIter].first[j]) && (!colors[curDomID].first[j])) {
						colors[curDomID].first[j] = true;
						colors[curDomID].second[j] = colors[*cIter].second[j] + 1;
						changeDone = true;
					}
					if ((allTrue) && (!colors[curDomID].first[j]))
						allTrue = false;
				}
				// by this time we've updated colors in descendant i of the domain *cIter and allTrue shows wheter this descendat covers all variables in the feature
				if (allTrue) // if we've found the root of the tree (its domainID is in curDomID)
					break;
				if (changeDone) // if coloring of curDomID domain was changed than inserting it into a set of newlyChanged domains
					newlyChanged.insert(curDomID);
			}
			if (allTrue) // if we've found the root of the tree (its domainID is in curDomID)
				break;
		}
		if (allTrue) // if we've found the root of the tree (its domainID is in curDomID)
			break;
		// updating recentlyChanged set
		recentlyChanged = newlyChanged;
	}
	// By this time we've found the root of the desired tree or stoped the algorithm since the graph is disjoint
	if (allTrue) { // if we've found the root of the tree
		unsigned int curLen = 0;
		std::vector <bool> unCovered;
		unCovered.resize(fSize, true);
		getPathFromRoot (path, curLen, curDomID, colors, unCovered);
		path.resize(curLen);
		return 1;
	} else {
		// if there is no tree in the graph that connects all required variables
		//  then creating and returning subsets of variables in the feature that are connected to each other
		std::vector <bool> covered;
		covered.resize(fSize, false);
		std::vector < std::pair<unsigned int, unsigned int> > curSubFeatDescr;
		for (unsigned int cVar = 0; cVar < fSize; cVar++) {
			if (covered[cVar])
				continue;
			curSubFeatDescr.clear();
			for (i = 0; i < fSize; i++) {
				if (colors[ clusterGraph.oneVarDomain [featureDescr[cVar].first] ].first[i]) {
					curSubFeatDescr.push_back(featureDescr[i]);
					covered[i] = true;
				}
			}
			subFeatureDescr.push_back(curSubFeatDescr);
		}
		return 2;
	}
	// Should not get here after previous 'if'
	return 0;
}


/****************************************************************************
*					LoopyBP::getPathFromRoot
*
*	Description:	A recursive function that creates path from root to the 
*					desired list domains
*	Parameters:		
*	Returns:		-
*
****************************************************************************/
void LoopyBP::getPathFromRoot ( std::vector < std::pair<unsigned int, bool> >& curPath, 
								unsigned int& curPathElem, 
								const unsigned int& curDomID, 
								std::map< unsigned int, std::pair <std::vector<bool>, std::vector<unsigned int> > >& colors, 
								std::vector<bool>& unCovered) const 
{
	unsigned int i, j, fSize, numToCover, crDomID, crSepSetID, crPathLen, curNeigh;
	fSize = unCovered.size();
	// Counting how many variables should be covered
	numToCover = 0;
	for (j = 0; j < fSize; j++) {
		if (unCovered[j]) {
			// checking if we've reached the domain that covers variable j
			if (colors[curDomID].second[j] == 0) { // means we've reached the desired domain of one required variable j
				unCovered[j] = false;
				continue;
			}
			numToCover++;
		}
	}
	// Trivial case when we do not need any more tree
	if (numToCover == 0) {
		return;
	}

	// Cycle by every variable that we need to cover
	//  vector for the results
	//        descendant domainID, description
	std::map <unsigned int, Triplet> domIDReqVars;
	for (i = 0; i < fSize; i++) {
		// if no need to cover a variable - just go for the next one
		if (!unCovered[i])
			continue;
		// Going through all neighbors and selecting one that covers variable i with the shortest path
		crPathLen = (unsigned int) -1;
		for (j = 0; j < clusterGraph.domainDescriptions[curDomID].neighbors.size(); j++) {
			// getting neighbor domainID
			if (curDomID == clusterGraph.sepSets[ clusterGraph.domainDescriptions[curDomID].neighbors[j] ].parOne)
				curNeigh = clusterGraph.sepSets[ clusterGraph.domainDescriptions[curDomID].neighbors[j] ].parTwo;
			else
				curNeigh = clusterGraph.sepSets[ clusterGraph.domainDescriptions[curDomID].neighbors[j] ].parOne;
			// If the current neighbor was not colored - skiping it
			if (colors[curNeigh].first.size() == 0)
				continue;
			// cheching whether this neighbor is the shortest path to the variable i original cluster
			if ((colors[curNeigh].first[i]) && (colors[curNeigh].second[i] < crPathLen)) {
				crPathLen = colors[curNeigh].second[i];
				crDomID = curNeigh;
				crSepSetID = clusterGraph.domainDescriptions[curDomID].neighbors[j];
			}		
		}
		if (crPathLen < ((unsigned int) -1)) { // if it was changed
			if (domIDReqVars[crDomID].reqVars.size() < 1) {
				domIDReqVars[crDomID].reqVars.resize(fSize, false);
				domIDReqVars[crDomID].sepSetID = crSepSetID;
				domIDReqVars[crDomID].dir = (curDomID == clusterGraph.sepSets[crSepSetID].parTwo);
			}
			domIDReqVars[crDomID].reqVars[i] = true;
		} else {
			// this printout could be removed after full testing
			std::cout << "ERROR in LoopyBP::getPathFromRoot: colored graph is inconsistent.\n";
		}
	}
	// by now 'domIDReqVars' includes all domains we'd like to include into the tree and specification of variables that we need from these subrees
	// going through all these domains and running recursion
	std::map <unsigned int, Triplet >::iterator it;
	for (it = domIDReqVars.begin(); it != domIDReqVars.end(); it++) {
		if (curPathElem >= curPath.size())
			curPath.resize(curPathElem + vectorMinResize);
		curPath[curPathElem].first = (*it).second.sepSetID;
		curPath[curPathElem].second = (*it).second.dir;
		curPathElem++;
		// running recursion starting from the selected domain
		getPathFromRoot (curPath, curPathElem, (*it).first, colors, (*it).second.reqVars);
	}
}


/****************************************************************************
*					LoopyBP::runPropagation
*
*	Description:	A recursive function that propagate beliefs along a tree 
*					that is constructed on the way
*	Parameters:		index - current index in sepSetSeq giving statring sepSetID
*	Returns:		-
*
****************************************************************************/
void LoopyBP::runPropagation (unsigned int& index, const bool& dir, PGMStruct& pgmStruct) {
	// sending a message over the sepSet
	sendMessage (sepSetSeq[index], dir, pgmStruct);
	// creating a vector of elements where we can go next
	unsigned int nextDomainID;
	if (dir) 
		nextDomainID = clusterGraph.sepSets[sepSetSeq[index]].parTwo;
	else
		nextDomainID = clusterGraph.sepSets[sepSetSeq[index]].parOne;
	std::vector <unsigned int> nextSepSetIDs;
	nextSepSetIDs.resize(clusterGraph.domainDescriptions[nextDomainID].neighbors.size() - 1);
	unsigned int numOfWays = clusterGraph.domainDescriptions[nextDomainID].neighbors.size();
	unsigned int i,j;
	// permutation of the vector 'nextSepSetIDs'
	std::vector <int> runOrder;
	runOrder.resize(numOfWays);
	for (i = 0; i < numOfWays; i++) {
		runOrder[i] = rand() % (numOfWays - i);
		for (j = 0; j < i; j++) {
			if (runOrder[i] >= runOrder[j])
				runOrder[i]++;
		}
	}
	// running recursion in the selected order
	unsigned int curNeigh, futureDom;
	for (i = 0; i < numOfWays; i++) {
		curNeigh = clusterGraph.domainDescriptions[nextDomainID].neighbors[ runOrder[i] ];
		if ( clusterGraph.sepSets[curNeigh].parOne == nextDomainID )
			futureDom = clusterGraph.sepSets[curNeigh].parTwo;
		else
			futureDom = clusterGraph.sepSets[curNeigh].parOne;
		if (( isSepSetFree[ curNeigh ] ) && ( isDomainFree[ futureDom ] )) {
			index++;
			sepSetSeq[index] = curNeigh;
			isSepSetFree[ curNeigh ] = false;
			isDomainFree[ nextDomainID ] = false;
			runPropagation(index, !dir, pgmStruct);
		}
	}
	// sending message back
	sendMessage (sepSetSeq[index], !dir, pgmStruct);
	index--;
}


/****************************************************************************
*					LoopyBP::runCondPropagation
*
*	Description:	Running message propagation along the constructed way in order 
*					to get expectation of the feature w.r.t. distribution defined by PGM
*	Parameters:		
*	Returns:		
*
****************************************************************************/
float LoopyBP::runCondPropagation ( unsigned int& oneVarDom, 
									std::vector < std::pair<unsigned int, bool> >& path, 
									const std::vector < std::pair<unsigned int, unsigned int> >& featureDescr, 
									const unsigned int& featureDescrSize,
									PGMStruct& pgmStruct) 
{
	// if path contain some elements - propagating messages
	unsigned int fSize = featureDescrSize;
	unsigned int j, k;
	// Trivial case when there is no need to propagate messages
	if (path.size() == 0) {
		// if the feature consists of one variable (no need to propagate messages)
		j = 0; 
		while ((j < fSize) && (featureDescr[j].first != oneVarDom)) 
			j++;
		return clusterGraph.domainDescriptions[oneVarDom].belief[featureDescr[j].second];
	}
	unsigned int curDom, nextDom = (unsigned int) -1; // domainID for the domain who should send and receive a message now
	std::map <unsigned int, std::vector <float>> newDomBeliefs;
	for (int i = path.size() - 1; i >= 0; i--) {
		// Identifying who should send a message next
		if (path[i].second) { // if direction is true - then sending a message from domain with one variable to the domain with many variables
			curDom = clusterGraph.sepSets[path[i].first].parOne;
			nextDom = clusterGraph.sepSets[path[i].first].parTwo;
		} else {
			curDom = clusterGraph.sepSets[path[i].first].parTwo;
			nextDom = clusterGraph.sepSets[path[i].first].parOne;
		}
		// checking whether we should update the domain (leave only values corresponding to featureDescr)
		if (path[i].second) {
			// checking whether curDom is in featureDescr
			j = 0; 
			while ((j < fSize) && (featureDescr[j].first != curDom)) 
				j++;
			// if it is in - then: 
			if (j < fSize) {
				//  multiplying domain belief by vector with only one 1 on the row that correspond to the value of the variable varInd in the feature descripttion
				if (newDomBeliefs[curDom].size() == 0) {
					newDomBeliefs[curDom].resize(clusterGraph.domainDescriptions[curDom].belief.size(), 0);
					newDomBeliefs[curDom][featureDescr[j].second] = clusterGraph.domainDescriptions[curDom].belief[featureDescr[j].second];
				} else {
					for (k = 0; k < newDomBeliefs[curDom].size(); k++) {
						if (k == j)
							continue;
						newDomBeliefs[curDom][k] = 0.0;
					}
				}
			}
		}
		// sending messages as usual
		if (newDomBeliefs[nextDom].size() == 0) 
			newDomBeliefs[nextDom] = clusterGraph.domainDescriptions[nextDom].belief;
		if (newDomBeliefs[curDom].size() == 0) 
			sendMessageConst (newDomBeliefs[nextDom], clusterGraph.domainDescriptions[curDom].belief, path[i].first, path[i].second, pgmStruct);
		else
			sendMessageConst (newDomBeliefs[nextDom], newDomBeliefs[curDom], path[i].first, path[i].second, pgmStruct);
	}
	// calculating sum of the belief of the root of the tree
	float toRet = 0.0;
	// nextDom - is the last updated belief (corresponding to the node of the tree)
	for (j = 0; j < newDomBeliefs[nextDom].size(); j++)
		toRet += newDomBeliefs[nextDom][j];
	return toRet;
}


/****************************************************************************
*					LoopyBP::selectSepSet
*
*	Description:	Picks a sepSet from a vector of sepSets in clusterGraph
*	Parameters:		chosenVal - ID of the selected sepSet in clusterGraph.sepSets
*					pgmStruct - description of the PGM
*	Returns:		bool - whether current state of domain beliefs is not calibrated
*
****************************************************************************/
 bool LoopyBP::selectSepSet (unsigned int& chosenVal, PGMStruct &pgmStruct) {
	// Pure random choise of sepSet
	//chosenVal = (unsigned int) ( ((double) rand() / ((double) RAND_MAX + 1)) * ((double) clusterGraph.sepSets.size()) );
	
	// defining whether to stop the LoopyBP
	std::vector <float> sumBel;
	unsigned int i, j;
	float aveSE, maxAveSE = acceptableDeviation;
	bool toRet = false;
	for (i = 0; i < clusterGraph.sepSets.size(); i++) {
		// calculating sepSet belief from point of view of the domain 'parTwo'
		sumBelief (clusterGraph.sepSets[i].parTwo, clusterGraph.sepSets[i].commonVar, pgmStruct, sumBel);
		normalizeMsgToAveOne (sumBel, sumBel);
		// calculating average squared error
		aveSE = 0;
		for (j = 0; j < sumBel.size(); j++)
			aveSE += pow (sumBel[j] - clusterGraph.domainDescriptions[clusterGraph.sepSets[i].parOne].belief[j], 2);
		aveSE /= (float) sumBel.size();
		// if average squared error is more that a threshold then return true
		if (aveSE > maxAveSE) {
			chosenVal = i;
			maxAveSE = aveSE;
			toRet = true;
			//return true;
		}
	}
	// printout of the max square error
	if (!toRet)
		std::cout << "\n\tAverage MSE = " << maxAveSE;
	// updating last Maximum Average Squared Error and lastMASEHolds if needed
	if (std::abs (1.0 - maxAveSE / lastMASE )  < eps) {
		lastMASEHolds++;
	} else {
		lastMASEHolds = 0;
	}
	/*if (maxAveSE > (lastMASE + eps) ) {
		std::cout << "\n\tIference: wrong direction.";
		return false;
	}*/
	lastMASE = maxAveSE;
	// if staying for too long in the same optimum - then stop trying
	if (lastMASEHolds > 2*pgmStruct.nVars ) {
		std::cout << "\n\tAverage MSE = " << maxAveSE;
		return false;
	}
	return toRet;
}


/****************************************************************************
*					LoopyBP::setZandNormalizeBeliefs
*
*	Description:	Function sets all marginals to be from 0 to 1 (divides by 
*					partition functions) and sets a partition function value
*	Parameters:		-
*	Returns:		float - partition function value
*
****************************************************************************/
float LoopyBP::setZandNormalizeBeliefs (void) {
	std::vector <bool> domainCovered;
	domainCovered.resize(clusterGraph.domainDescriptions.size(), false);
	clusterGraph.Z = 1.0;
	for (unsigned int i = 0; i < clusterGraph.oneVarDomain.size(); i++) {
		// selecting first one variable domain that is not yet covered
		if (domainCovered [clusterGraph.oneVarDomain[i]])
			continue;
		clusterGraph.Z *= normalizeBelief ((unsigned int) clusterGraph.oneVarDomain[i], domainCovered);
	}
	return clusterGraph.Z;
}


/****************************************************************************
*					LoopyBP::setParameters
*
*	Description:	Recursive function that normalize belief for domain curDomID 
*					and all its sepSets and runs itself for all its descendatns
*	Parameters:		curDomID - Id of the domain where to start normalizing beliefs
*					domainCovered - stores 'true' for domains that were visited by
*						this function
*	Returns:		float - partition function value for the last updated belief
*
****************************************************************************/
float LoopyBP::normalizeBelief (const unsigned int& curDomID, std::vector<bool>& domainCovered) {
	float curZ = 0.0;
	unsigned int i, j, curNeigh, nextDom;
	// counting current Z
	for (i = 0; i < clusterGraph.domainDescriptions[curDomID].belief.size(); i++ )
		curZ += clusterGraph.domainDescriptions[curDomID].belief[i];
	// scaling marginal of the domain
	for (i = 0; i < clusterGraph.domainDescriptions[curDomID].belief.size(); i++ )
		clusterGraph.domainDescriptions[curDomID].belief[i] /= curZ;
	// setting vector element to be true - means that we've gone through domain curDomID
	domainCovered[curDomID] = true;
	// running normalization for all connected unvisited domains
	for (i = 0; i < clusterGraph.domainDescriptions[curDomID].neighbors.size(); i++) {
		curNeigh = clusterGraph.domainDescriptions[curDomID].neighbors[i];
		if (curDomID == clusterGraph.sepSets[curNeigh].parOne) 
			nextDom = clusterGraph.sepSets[curNeigh].parTwo;
		else
			nextDom = clusterGraph.sepSets[curNeigh].parOne;
		// if domain was visited - going to the next one
		if (domainCovered[nextDom])
			continue;
		// scaling marginals of the connected sepSet
		for (j = 0; j < clusterGraph.sepSets[curNeigh].muOneTwo.size(); j++) {
			clusterGraph.sepSets[curNeigh].muOneTwo[j] /= curZ;
			clusterGraph.sepSets[curNeigh].muTwoOne[j] /= curZ;
		}
		// running recursion
		normalizeBelief (nextDom, domainCovered);
	}
	// returning current Z value
	return curZ;
}


/****************************************************************************
*					LoopyBP::setParameters
*
*	Description:	Set object patameters from environment
*	Parameters:		environment - a set of parameters read from config file
*	Returns:		0
*
****************************************************************************/
int LoopyBP::setParameters (Environment &environment) {
	double valD;
	if (environment.getDoubleParamValue(valD, "LoopyBP_DampingParameter") == 0)
		lambda = (float) valD;
	if (environment.getDoubleParamValue(valD, "LoopyBP_AverageMaxDeviation") == 0)
		acceptableDeviation = (float) valD;
	return 0;
}
